#include "../include/KMF.h"
#include <iostream>
#include <Eigen/Dense>

int main()
{
    Eigen::MatrixXf F(2,2);
    F<<1.0f,0.1f,0.0f,1.0f;
    Eigen::MatrixXf H(1,2);
    H<<1.0f,0.0f;
    Eigen::MatrixXf R(1,1);
    R<<0.1f;
    Eigen::MatrixXf Q(2,2);
    Q<<0.5f,0.0f,0.5f,0.0f;
    Eigen::MatrixXf B(2,1);
    B<<0.1,0;

    Eigen::VectorXf U(1);
    U<<0.2;
    Eigen::VectorXf X(2);
    X<<1.2,0.03;
    Eigen::VectorXf Z(1);
    Z<<1.3;

    dip::KalmanFilter filter;
    filter.print_debug_string();
    filter.setF(F);
    filter.setH(H);
    filter.setQ(Q);
    filter.setR(R);
    filter.reset(X);
    
    filter.print_debug_string();
    filter.setB(B);
    filter.print_debug_string();

    filter.update(Z);

    Eigen::MatrixXf cov;
    filter.getCov(cov);
    std::cout<<"cov:"<<cov<<std::endl;
    filter.getState(X);
    std::cout<<"filter x:"<<X<<std::endl;
    filter.predict(X);
    std::cout<<"predict x without u"<<X<<std::endl;
    filter.predict(U,X);
    std::cout<<"predict with u"<<X<<std::endl;

    return 0;

}